#include <robot/robot.h>

#include <robot/sdh/chandclient.h>
#include <blackbox/connect.h>

namespace buola { namespace robot { namespace sdh {

CHandClient::CHandClient()
    :   BBox("hand client")
    ,   iJoints(this,"joints in",7)
    ,   iStatus(this,"status in")
{
}

CHandClient::~CHandClient()
{
}

void CHandClient::Init(const blackbox::CBoxProxy &pBox)
{
    mHand.reset(new blackbox::CBoxProxy(pBox));
    bb_connect(pBox["joints out"],iJoints);
    bb_connect(pBox["status out"],iStatus);
}

void CHandClient::GetPose(mat::CVecNd<7> &pValues)
{
    PullAtomicInputs();
    for(int i=0;i<7;i++)
        pValues[i]=iJoints.Get(i);
}

void CHandClient::SetGoal(const mat::CVecNd<7> &pJoints)
{
    mHand->Call<void(mat::CVecNd<7>)>("set goal",pJoints);
}

void CHandClient::SetGoal(const double *pJoints)
{
    SetGoal(mat::CVecNd<7>(pJoints));
}

void CHandClient::SetGoal(int pJoint,double pValue)
{
    mat::CVecNd<7> lValues;
    GetPose(lValues);
    lValues[pJoint]=pValue;
    SetGoal(lValues);
}

void CHandClient::SetCurrent(double pValue)
{
    mat::CVecNd<7> lValues;
    for(int i=0;i<7;i++) lValues[i]=pValue;
        mHand->Call<void(mat::CVecNd<7>)>("set current",lValues);
}

void CHandClient::Stop()
{
    mat::CVecNd<7> lCurrent;
    GetPose(lCurrent);
    SetGoal(lCurrent);
}

bool CHandClient::IsMoving()
{
    PullAtomicInputs();
    return iStatus.Get();
}

void CHandClient::WaitForStop()
{
    int lCount=0;
    while(lCount<5)
    {
        if(!IsMoving()) lCount++;
        sleep(chrono::milliseconds(10));
    }
}

/*namespace kuka*/ } /*namespace robot*/ } /*namespace buola*/ }
